#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Hubs,  S2, HTServo,  HTMotor,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Sensor, S3,     tLimits,        sensorI2CCustom9V)
#pragma config(Sensor, S4,     tIRBeacon,      sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  motorA,          mTow,          tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorB,          mLeftTow,      tmotorNXT, PIDControl, encoder)
#pragma config(Motor,  motorC,          mRightTow,     tmotorNXT, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C1_1,     dLeft1,        tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     dLeft2,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     dRight1,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     dRight2,       tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C2_1,     aBallTip,      tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C2_2,     aUExtend,      tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_1,     aBallPickup,   tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S2_C3_2,     aLExtend,      tmotorTetrix, openLoop, reversed)
#pragma config(Servo,  srvo_S2_C1_1,    sBallDrop,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_2,    sIRSeeker,            tServoStandard)
#pragma config(Servo,  srvo_S2_C1_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S2_C1_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "FTC_Toolkit.h"

// IR Beacon Read Position
//int Beacon_Locs[2] = {2500, 8000};
//int Turn_Points[3] = {2000, 5000, 8000};
//int First_Turn[3] = {DEG_180_TURN, DEG_90_TURN, DEG_45_TURN};

int IRVal = 0;

void initializeRobot()
{
  // Place code here to sinitialize servos to starting positions.
  // Sensors are automatically configured and setup by ROBOTC. They may need a brief time to stabilize.
	motor[dLeft1] = motor[dLeft2] = motor[dRight1] = motor[dRight2] = 0;
  motor[aBallTip] = motor[aUExtend] = motor[aBallPickup] = motor[aLExtend] = 0;
  servo[sBallDrop] = 255;
  servo[sIRSeeker] = IR_SERVO_DOWN;
  ImplementVariables();
	wait1Msec(300);

  return;
}

void TestIR(void){
	servo[sIRSeeker] = IR_SERVO_UP;
	while(true){
		IRVal = SensorValue[tIRBeacon];
	}
}

task main()
{
  initializeRobot();

  //waitForStart(); // Wait for the beginning of autonomous phase.
  // Move to start
  StartTask(ReleaseBoom);
  waitForLimit(lLOWEXTEND_LOW, false);

  // Drive to check position
  EncoderDrive(-30, 0, 2000, true);
  EncoderDrive( 0, -50, DEG_45_TURN, true);
  EncoderDrive( -30, 0, 2000, true);
  EncoderDrive(0, 50, DEG_90_TURN, true);
  waitForTask(&tReleaseBoom_Active);
  EncoderDrive( -50, 0, 700, true);

	// Check IR Beacon
  wait1Msec(250);
	IRVal = ReadIRBeacon();
	wait1Msec(250);

	// Movqe Back
	EncoderDrive( 50, 0, 1000, true);

	// Move According to Beacon Position
	if (IRVal < 5){
		// Left of Robot - Knock Down Pole

	}
	else if (IRVal ==  5){
		// Center to robot
		StartTask(PositionHighGoal);
		EncoderDrive(0, -50, DEG_90_TURN, true);
		EncoderDrive(30, 0, 500, true);
		EncoderDrive(0, 50, DEG_90_TURN, true);
	}
	else {
		// Right of Robot
		EncoderDrive(0, -50, DEG_45_TURN, true);
		EncoderDrive(30, 0, 850, true);
		EncoderDrive(0, -50, DEG_90_TURN, true);
		EncoderDrive(30, 0, 3400, true);
		StartTask(PositionHighGoal);
		EncoderDrive(0, 50, DEG_90_TURN + 30, true);
	}

	waitForTask(&tPositionHighGoal_Active);
	StartTask(ScoreBall);
	waitForTask(&tScoreBall_Active);
	/*

	// Move Goal Up
	StartTask(PositionHighGoal);
  waitForTask(&tPositionHighGoal_Active);

	// Move Into
	DriveSystem(-40, 0);
	wait1Msec(1500);
	DriveSystem(0, 0);
	wait1Msec(250);

	// Score Ball
	StartTask(ScoreBall);
	waitForTask(&tScoreBall_Active);
	// Drive Out and Fix Goal Grab
	EncoderDrive(30, 0, 2000, true);
	StartTask(HighExtendDown);

  // Drive to Pin
	EncoderDrive(0, 60, DEG_45_TURN + 25, true);	// Turn Left
	EncoderDrive(-50, 0, 2250, true);					// Clear Corner
	EncoderDrive(0, -60, DEG_90_TURN - 250, true);		// Align to Pin Edge
	EncoderDrive(-100, 0, 3500, true);					// POWER! POWER!

  // Cause Mistif
*/
  while (true)
  {}
}
